﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Drawing;
using System.Windows.Forms;
using MotionPlanning.MotionPlanningComponents.MotionPlanningSystems;
using MotionPlanning.MotionPlanningPrimitives;
using MotionPlanning.MotionPlanningAlgorithms;
using MotionPlanning.MotionPlanningComponents.Visualizations;
using System.Diagnostics;

namespace MotionPlanning.MotionPlanningComponents.Visualizations
{
    //
    // Visualizer for System_OneLink
    //
    public class Visualizer_OneLink : Visualizer
    {
        SolidBrush brushNodePlot;
        SolidBrush brushStateRed;
        Pen edgeBlue;
        Pen edgeBlueVirtual;

        public Visualizer_OneLink(Graphics canvas, Rectangle rect)
            : base(canvas, rect)
        {
            this.canvas = canvas;
            this.canvasRect = new Rectangle(0, 0, rect.Width, rect.Height);
            brushCanvasClear = new SolidBrush(Color.FromArgb(255,255,255));
            brushNodePlot = new SolidBrush(Color.Red);
            canvasAxisGray = new Pen(Color.Gray);
            edgeBlue = new Pen(Color.Blue, 2);
            edgeBlueVirtual = new Pen(Color.Red, 2);
            brushStateRed = new SolidBrush(Color.Red);
            calibrater_x = canvasRect.Width / 2;
            calibrater_y = -canvasRect.Height / 2;
            center_x = rect.Width / 2;
            center_y = rect.Height / 2;
            screen_width = rect.Width;
            screen_height = rect.Height;
        }

        override public void PlotVerticesWithCost(List<object> train)
        {
            double[] q1p = new double[2];
            for (int i = 0; i < train.Count; ++i)
            {
                Node cn = ((Node)train[i]);
                double cost = cn.get_Value().get_utility_c();
                double[] q1 = cn.get_Value().get_q();
                double x_tmp = q1[0];
                while (x_tmp > Math.PI)
                    x_tmp -= Math.PI * 2;
                while (x_tmp < -Math.PI)
                    x_tmp += Math.PI * 2;
                int screen_x1 = (int)(x_tmp * this.calibrater_x + center_x);
                int screen_y1 = (int)(q1[1] * this.calibrater_y + center_y);


            }

            canvas.DrawLine(canvasAxisGray, 0, center_y, screen_width, center_y);
            canvas.DrawLine(canvasAxisGray, center_x, 0, center_x, screen_height);
        }

        public void PlotVerticesWithCost_backup(List<object> train)
        {
            double distance_threshold2 = 200 * 200;
            double[] q1p = new double[2];
            double[] q2p = new double[2];
            for (int i = 0; i < train.Count; ++i)
            {
                List<Edge> pe = ((Node)train[i]).get_Parents();
                if (pe == null) continue;

                for (int j = pe.Count - 1; j < pe.Count; ++j)
                {
                    
                    Edge p = pe[j];
                    double[] q1 = p.get_entry().get_Value().get_q();
                    double[] q2 = p.get_exit().get_Value().get_q();
                    double cost = p.get_exit().get_Value().get_utility_c();
                    Pen edgePen = new Pen(Cost2Color(cost, 7));

                    int screen_x1 = (int)(q1[0] * this.calibrater_x + center_x);
                    int screen_y1 = (int)(q1[1] * this.calibrater_y + center_y);

                    int screen_x2 = (int)(q2[0] * this.calibrater_x + center_x);
                    int screen_y2 = (int)(q2[1] * this.calibrater_y + center_y);

                    double dx = screen_x1 - screen_x2;
                    double dy = screen_y1 - screen_y2;
                    double distance2 = dx * dx + dy * dy;
                    if (distance2 > distance_threshold2 && q2[0] * q1[0] < 0)
                    {
                        q2p[0] = q2[0];
                        q2p[1] = q2[1];
                        if (q2p[0] < 0)
                            q2p[0] += Math.PI + Math.PI;
                        else
                            q2p[0] -= Math.PI + Math.PI;
                        int screen_x2p = (int)(q2p[0] * this.calibrater_x + center_x);
                        int screen_y2p = (int)(q2p[1] * this.calibrater_y + center_y);
                        canvas.DrawLine(edgePen, screen_x1, screen_y1, screen_x2p, screen_y2p);

                        q1p[0] = q1[0];
                        q1p[1] = q1[1];
                        if (q1p[0] < 0)
                            q1p[0] += Math.PI + Math.PI;
                        else
                            q1p[0] -= Math.PI + Math.PI;
                        int screen_x1p = (int)(q1p[0] * this.calibrater_x + center_x);
                        int screen_y1p = (int)(q1p[1] * this.calibrater_y + center_y);
                        canvas.DrawLine(edgePen, screen_x1p, screen_y1p, screen_x2, screen_y2);
                    }
                    else
                        canvas.DrawLine(edgePen, screen_x1, screen_y1, screen_x2, screen_y2);
                }
            }
        }

        //probing
        private double ScreenDistance(double x1, double y1, double x2, double y2)
        {
            double dx = x1 - x2;
            double dy = y1 - y2;
            return Math.Sqrt(dx * dx + dy * dy);
        }

        private Color Cost2Color(double cost, double cutOff)
        {
            if (cost > cutOff)
                return Color.Red;
            double relativeRatio = cost / cutOff;
            return Color.FromArgb(0, (int)(255 - 128 * relativeRatio), 0);
        }

        override public void PlotEdges(List<object> train)
        {
            double distance_threshold2 = 200 * 200;
            double[] q1p = new double[2];
            double[] q2p = new double[2];
            for (int i = 0; i < train.Count; ++i)
            {
                List<Edge> pe = ((Node)train[i]).get_Parents();
                if (pe == null) continue;

                for (int j = pe.Count - 1; j < pe.Count; ++j)
                {
                    Pen edgePen = edgeBlueVirtual;
                    if (j == 0)
                    {
                        edgePen = edgeBlue;
                        //continue;  //comment this line to show RRT edges
                                        //uncomment this line to show RDG edges without RRT edges
                    }
                    Edge p = pe[j];
                    double[] q1 = p.get_entry().get_Value().get_q();
                    double[] q2 = p.get_exit().get_Value().get_q();

                    int screen_x1 = (int)(q1[0] * this.calibrater_x + center_x);
                    int screen_y1 = (int)(q1[1] * this.calibrater_y + center_y);

                    int screen_x2 = (int)(q2[0] * this.calibrater_x + center_x);
                    int screen_y2 = (int)(q2[1] * this.calibrater_y + center_y);

                    double dx = screen_x1 - screen_x2;
                    double dy = screen_y1 - screen_y2;
                    double distance2 = dx * dx + dy * dy;
                    if (distance2 > distance_threshold2 && q2[0] * q1[0] < 0)
                    {
                        q2p[0] = q2[0];
                        q2p[1] = q2[1];
                        if (q2p[0] < 0)
                            q2p[0] += Math.PI + Math.PI;
                        else
                            q2p[0] -= Math.PI + Math.PI;
                        int screen_x2p = (int)(q2p[0] * this.calibrater_x + center_x);
                        int screen_y2p = (int)(q2p[1] * this.calibrater_y + center_y);
                        canvas.DrawLine(edgePen, screen_x1, screen_y1, screen_x2p, screen_y2p);

                        q1p[0] = q1[0];
                        q1p[1] = q1[1];
                        if (q1p[0] < 0)
                            q1p[0] += Math.PI + Math.PI;
                        else
                            q1p[0] -= Math.PI + Math.PI;
                        int screen_x1p = (int)(q1p[0] * this.calibrater_x + center_x);
                        int screen_y1p = (int)(q1p[1] * this.calibrater_y + center_y);
                        canvas.DrawLine(edgePen, screen_x1p, screen_y1p, screen_x2, screen_y2);
                    }
                    else
                        canvas.DrawLine(edgePen, screen_x1, screen_y1, screen_x2, screen_y2);
                }
            }
        }

    }
}
